/*    

    bobbietalk - a visual programming environment for toy robotics

    Copyright (C) 2013  Roberto Tiella

    This program is free software: you can redistribute it and/or modify
    it under the terms of the GNU General Public License as published by
    the Free Software Foundation, either version 3 of the License, or
    (at your option) any later version.

    This program is distributed in the hope that it will be useful,
    but WITHOUT ANY WARRANTY; without even the implied warranty of
    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
    GNU General Public License for more details.

    You should have received a copy of the GNU General Public License
    along with this program.  If not, see <http://www.gnu.org/licenses/>.

*/
package bobbietalk.simulator.controllers;

import org.apache.log4j.Logger;

/**
 * A basic implementation of a controller. It implements actions such as
 * forward,backward,left and right controlled by rotation sensors on axes
 *
 * @author tiella
 */
public abstract class BasicAbstractController extends AbstractController {

    private static Logger log = Logger.getLogger(BasicAbstractController.class);
    private double defautSpeed = 5;

    protected void backward(int tics) throws InterruptedException {

        theRobot().leftMotorBackward();
        theRobot().rightMotorBackward();

        waitMovementCompleted(tics);

        theRobot().leftMotorOff();
        theRobot().rightMotorOff();
    }

    protected void forward(int tics) throws InterruptedException {

        theRobot().leftMotorForward();
        theRobot().rightMotorForward();

        waitMovementCompleted(tics);

        theRobot().leftMotorOff();
        theRobot().rightMotorOff();
    }

    protected void left(int tics) throws InterruptedException {

        theRobot().leftMotorBackward();
        theRobot().rightMotorForward();

        waitMovementCompleted(tics);

        theRobot().leftMotorOff();
        theRobot().rightMotorOff();
    }

    protected void right(int tics) throws InterruptedException {

        theRobot().leftMotorForward();
        theRobot().rightMotorBackward();


        waitMovementCompleted(tics);

        theRobot().leftMotorOff();
        theRobot().rightMotorOff();
    }

    private void waitMovementCompleted(int tics) throws InterruptedException {

        int firstR = theRobot().getTickR();
        int lastR = firstR;

        while (true) {

            Thread.sleep(100);

            final int tickR = theRobot().getTickR();

            if (tickR == lastR || Math.abs(tickR - firstR) >= tics) {
                break;
            }

            lastR = tickR;
        }
    }

    protected void setDefaultSpeed(double s) {
        defautSpeed = s;
    }

    protected void forward() {
        theRobot().setSpeed(defautSpeed);
        theRobot().leftMotorForward();
        theRobot().rightMotorForward();
    }

    protected void backward() {
        theRobot().setSpeed(defautSpeed);
        theRobot().leftMotorBackward();
        theRobot().rightMotorBackward();
    }

    protected void left() {
        theRobot().setSpeed(defautSpeed);
        theRobot().leftMotorBackward();
        theRobot().rightMotorForward();
    }

    protected void off() {
        theRobot().leftMotorOff();
        theRobot().rightMotorOff();
        log.info("motors stopped");
    }

    protected void right() {
        theRobot().setSpeed(defautSpeed);
        theRobot().leftMotorForward();
        theRobot().rightMotorBackward();
    }
}
